/**
 * @file Fwaypoints.c
 * @brief 飞机航点生成系统 - 智能优化版本
 * @author Jim
 * @date 2025
 *
 * 功能：基于网格地图生成优化的无人机航点
 * 策略：无障碍行只生成起点终点，有障碍行生成详细绕行航点
 * 输出：无人机坐标系的实际坐标
 */

#include "Fwaypoints.h"
#include "2025E.h"

// 全局变量
static DroneWaypoint_t flight_waypoints[MAX_FLIGHT_WAYPOINTS];
static uint16_t flight_waypoint_count = 0;
     DroneWaypoint_t test_waypoints[MAX_FLIGHT_WAYPOINTS];
uint16_t waypoint_count = 0;
/**
 * @brief 网格坐标转换为无人机坐标系
 * @param row 网格行号 (0-6)
 * @param col 网格列号 (0-8)
 * @param drone_x 返回无人机X坐标
 * @param drone_y 返回无人机Y坐标
 */
void ConvertToDroneCoordinate(uint8_t row, uint8_t col, float *drone_x, float *drone_y)
{
    // 坐标转换：网格坐标 → 无人机坐标系
    // 网格(0,0)对应B1A1，网格(6,8)对应B7A9
    *drone_x = (col - 8) * DRONE_GRID_SIZE_CM;
    // Y坐标：B1(row=0)=0, B7(row=6)=300  
    *drone_y = row * DRONE_GRID_SIZE_CM;
}

/**
 * @brief 添加航点到列表
 * @param row 网格行号
 * @param col 网格列号
 * @param type 航点类型
 * @return 成功返回1，失败返回0
 */
uint8_t AddFlightWaypoint(uint8_t row, uint8_t col, WaypointType_t type)
{
    if (flight_waypoint_count >= MAX_FLIGHT_WAYPOINTS)
        return 0;
     if(row>6||col>8)
        return 0;
    // 转换坐标
    ConvertToDroneCoordinate(row, col,
                           &flight_waypoints[flight_waypoint_count].x,
                           &flight_waypoints[flight_waypoint_count].y);

    flight_waypoints[flight_waypoint_count].type = type;
    flight_waypoints[flight_waypoint_count].grid_row = row;
    flight_waypoints[flight_waypoint_count].grid_col = col;

    flight_waypoint_count++;
    return 1;
}

/**
 * @brief 分析指定行的禁飞区分布
 * @param row 行号 (0-6)
 * @return 行分析结果
 */
RowAnalysis_t AnalyzeRowObstacles(uint8_t row)
{
    RowAnalysis_t analysis = {0};

    if (row >= GRID_ROWS)
        return analysis;

    // 扫描整行，查找禁飞区
    for (uint8_t col = 0; col < GRID_COLS; col++)
    {
        if (!IsGridAccessible(row, col))
        {
            analysis.has_obstacles = 1;

            if (analysis.obstacle_count == 0)
            {
                analysis.first_obstacle_col = col;
            }
            analysis.last_obstacle_col = col;
            analysis.obstacle_count++;
        }
    }

    return analysis;
}

/**
 * @brief 为无障碍行生成简化航点（只有起点和终点）
 * @param row 行号
 * @return 成功返回1，失败返回0
 */
uint8_t GenerateSimpleRowWaypoints(uint8_t row)
{
    if (row % 2 == 0) // 偶数行：从右到左 (A9→A1)
    {
        // 起点：右侧 (A9)
        if (!AddFlightWaypoint(row, GRID_COLS - 1, WAYPOINT_NORMAL))
            return 0;

        // 终点：左侧 (A1)
        if (!AddFlightWaypoint(row, 0, WAYPOINT_NORMAL))
            return 0;
    }
    else // 奇数行：从左到右 (A1→A9)
    {
        // 起点：左侧 (A1)
        if (!AddFlightWaypoint(row, 0, WAYPOINT_NORMAL))
            return 0;

        // 终点：右侧 (A9)
        if (!AddFlightWaypoint(row, GRID_COLS - 1, WAYPOINT_NORMAL))
            return 0;
    }

    return 1;
}

/**
 * @brief 为有障碍行生成详细绕行航点（修正B1行向上绕行和边缘禁飞区处理）
 * @param row 行号
 * @param analysis 行分析结果
 * @return 成功返回1，失败返回0
 */
uint8_t GenerateDetailedRowWaypoints(uint8_t row, RowAnalysis_t analysis)
{
    // 在函数开头定义所有变量，避免重复定义
    uint8_t detour_row;
    uint8_t bypass_start_col;

    if (row % 2 == 0) // 偶数行：从右到左
    {
        // 特殊情况：偶数行最左边禁飞区
        if (analysis.first_obstacle_col == 0)
        {
            if (row == GRID_ROWS - 1) // B7行A1禁飞区
            {
                // A1B7是最终终点，直接停止
                AddFlightWaypoint(row, GRID_COLS - 1, WAYPOINT_NORMAL); // 起点A9
                if (analysis.last_obstacle_col < GRID_COLS - 1)
                {
                    AddFlightWaypoint(row, analysis.last_obstacle_col + 1, WAYPOINT_NORMAL); // 禁飞区前停止
                }
                return 1;
            }
            else
            {
                // 其他偶数行：向上绕行，两段式
                AddFlightWaypoint(row, GRID_COLS - 1, WAYPOINT_NORMAL); // 起点A9
                AddFlightWaypoint(row, analysis.last_obstacle_col + 1, WAYPOINT_NORMAL); // 禁飞区前
                AddFlightWaypoint(row + 1, analysis.last_obstacle_col + 1, WAYPOINT_BYPASS); // 向上
                AddFlightWaypoint(row + 1, 0, WAYPOINT_BYPASS); // 水平到A1
                return 1; // 不回到原行
            }
        }

        // 1. 正常扫描到禁飞区前
        if (analysis.last_obstacle_col < GRID_COLS - 1)
        {
            AddFlightWaypoint(row, GRID_COLS - 1, WAYPOINT_NORMAL); // 起点
            AddFlightWaypoint(row, analysis.last_obstacle_col + 1, WAYPOINT_NORMAL); // 禁飞区前
        }

        // 检查A1B7特殊情况：如果是B7行且A1是禁飞区，直接停止
        if (row == GRID_ROWS - 1 && analysis.first_obstacle_col == 0)
        {
            return 1; // 直接停止，不继续生成航点
        }

        // 2. 三段式绕行逻辑
        if (row == 0) // B1行特殊处理：向上绕行
        {
            detour_row = row + 1; // 向上到B2行
        }
        else // 其他行：向下绕行
        {
            detour_row = row - 1; // 向下绕行
        }

        // 检查绕行起点是否超出地图边界
        bypass_start_col = analysis.last_obstacle_col + 1;
        if (bypass_start_col >= GRID_COLS)
        {
            // 禁飞区延伸到右边缘，从禁飞区左边开始绕行
            if (analysis.first_obstacle_col > 0)
            {
                // 直接从禁飞区左边开始绕行
                AddFlightWaypoint(detour_row, analysis.first_obstacle_col - 1, WAYPOINT_BYPASS);

                // 垂直回到原行
                AddFlightWaypoint(row, analysis.first_obstacle_col - 1, WAYPOINT_BYPASS);

                // 继续正常扫描到终点
                AddFlightWaypoint(row, 0, WAYPOINT_NORMAL); // 终点
            }
            return 1;
        }
        else
        {
            // 正常三段式绕行：垂直移动到绕行行
            AddFlightWaypoint(detour_row, bypass_start_col, WAYPOINT_BYPASS);

            // 水平跳过禁飞区
            if (analysis.first_obstacle_col > 0)
            {
                AddFlightWaypoint(detour_row, analysis.first_obstacle_col - 1, WAYPOINT_BYPASS);

                // 垂直回到原行
                AddFlightWaypoint(row, analysis.first_obstacle_col - 1, WAYPOINT_BYPASS);
            }

            // 3. 继续正常扫描到终点
            if (analysis.first_obstacle_col > 0)
            {
                AddFlightWaypoint(row, 0, WAYPOINT_NORMAL); // 终点
            }

            return 1; // 三段式绕行完成，立即退出
        }

        // 2. 绕行航点序列 - 修正B1行特殊处理
        uint8_t detour_row;
        if (row == 0) // B1行特殊处理：向上绕行
        {
            detour_row = row + 1; // 向上到B2行
        }
        else // 其他行：向下绕行
        {
            detour_row = row - 1; // 向下绕行
        }

        // 检查绕行起点是否超出地图边界
        uint8_t bypass_start_col = analysis.last_obstacle_col + 1;
        if (bypass_start_col >= GRID_COLS)
        {
            // 禁飞区延伸到右边缘，从禁飞区左边开始绕行
            if (analysis.first_obstacle_col > 0)
            {
                // 直接从禁飞区左边开始绕行
                AddFlightWaypoint(detour_row, analysis.first_obstacle_col - 1, WAYPOINT_BYPASS);

                // 垂直回到原行
                AddFlightWaypoint(row, analysis.first_obstacle_col - 1, WAYPOINT_BYPASS);

                // 继续正常扫描到终点
                AddFlightWaypoint(row, 0, WAYPOINT_NORMAL); // 终点
            }
            return 1;
        }

        // 正常绕行：垂直移动到绕行行
        AddFlightWaypoint(detour_row, bypass_start_col, WAYPOINT_BYPASS);

        // 水平跳过禁飞区
        if (analysis.first_obstacle_col > 0)
        {
            AddFlightWaypoint(detour_row, analysis.first_obstacle_col - 1, WAYPOINT_BYPASS);

            // 垂直回到原行
            AddFlightWaypoint(row, analysis.first_obstacle_col - 1, WAYPOINT_BYPASS);
        }

        // 3. 继续正常扫描到终点
        if (analysis.first_obstacle_col > 0)
        {
            AddFlightWaypoint(row, 0, WAYPOINT_NORMAL); // 终点
        }
    }
    else // 奇数行：从左到右
    {
        // 特殊情况：奇数行最右边禁飞区
        if (analysis.last_obstacle_col == GRID_COLS - 1)
        {
            // 向上绕行，两段式
            AddFlightWaypoint(row, 0, WAYPOINT_NORMAL); // 起点A1
            AddFlightWaypoint(row, analysis.first_obstacle_col - 1, WAYPOINT_NORMAL); // 禁飞区前
            AddFlightWaypoint(row + 1, analysis.first_obstacle_col - 1, WAYPOINT_BYPASS); // 向上
            AddFlightWaypoint(row + 1, GRID_COLS - 1, WAYPOINT_BYPASS); // 水平到A9
            return 1; // 不回到原行
        }

        // 1. 正常扫描到禁飞区前
        if (analysis.first_obstacle_col > 0)
        {
            AddFlightWaypoint(row, 0, WAYPOINT_NORMAL); // 起点
            AddFlightWaypoint(row, analysis.first_obstacle_col - 1, WAYPOINT_NORMAL); // 禁飞区前
        }

        // 2. 三段式绕行逻辑
        if (row == 0) // B1行特殊处理：向上绕行
        {
            detour_row = row + 1; // 向上到B2行
        }
        else // 其他行：向下绕行
        {
            detour_row = row - 1; // 向下绕行
        }

        // 检查绕行起点是否超出地图边界
        if (analysis.first_obstacle_col == 0)
        {
            // 禁飞区延伸到左边缘，从禁飞区右边开始绕行
            if (analysis.last_obstacle_col < GRID_COLS - 1)
            {
                // 直接从禁飞区右边开始绕行
                AddFlightWaypoint(detour_row, analysis.last_obstacle_col + 1, WAYPOINT_BYPASS);

                // 垂直回到原行
                AddFlightWaypoint(row, analysis.last_obstacle_col + 1, WAYPOINT_BYPASS);

                // 继续正常扫描到终点
                AddFlightWaypoint(row, GRID_COLS - 1, WAYPOINT_NORMAL); // 终点
            }
            return 1;
        }
        else
        {
            // 正常三段式绕行：垂直移动到绕行行
            AddFlightWaypoint(detour_row, analysis.first_obstacle_col - 1, WAYPOINT_BYPASS);

            // 水平跳过禁飞区
            if (analysis.last_obstacle_col < GRID_COLS - 1)
            {
                AddFlightWaypoint(detour_row, analysis.last_obstacle_col + 1, WAYPOINT_BYPASS);

                // 垂直回到原行
                AddFlightWaypoint(row, analysis.last_obstacle_col + 1, WAYPOINT_BYPASS);
            }

            // 3. 继续正常扫描到终点
            if (analysis.last_obstacle_col < GRID_COLS - 1)
            {
                AddFlightWaypoint(row, GRID_COLS - 1, WAYPOINT_NORMAL); // 终点
            }

            return 1; // 三段式绕行完成，立即退出
        }
    }

    return 1;
}

/**
 * @brief 生成行间转换航点（避免重复）
 * @param from_row 起始行
 * @param to_row 目标行
 * @return 成功返回1，失败返回0
 */
uint8_t GenerateRowTransitionWaypoint(uint8_t from_row, uint8_t to_row)
{
    // 确定转换点的列位置
    uint8_t transition_col;

    if (from_row % 2 == 0) // 偶数行结束在左侧
    {
        transition_col = 0; // A1
    }
    else // 奇数行结束在右侧
    {
        transition_col = GRID_COLS - 1; // A9
    }

    // 检查是否与下一行的起点重复
    uint8_t next_row_start_col;
    if (to_row % 2 == 0) // 目标行是偶数行，从右开始
    {
        next_row_start_col = GRID_COLS - 1; // A9
    }
    else // 目标行是奇数行，从左开始
    {
        next_row_start_col = 0; // A1
    }

    // 如果转换点与下一行起点相同，则不添加转换航点（避免重复）
    if (transition_col == next_row_start_col)
    {
        return 1; // 不添加，但返回成功
    }

    // 添加转换航点
    return AddFlightWaypoint(to_row, transition_col, WAYPOINT_TRANSITION);
}

/**
 * @brief 生成优化的横向巡航航点（主函数）
 * @param waypoints 输出航点数组
 * @param max_count 最大航点数
 * @return 生成的航点数量
 */
uint16_t GenerateOptimizedHorizontalWaypoints(DroneWaypoint_t *waypoints, uint16_t max_count)
{
    // 重置航点计数
    flight_waypoint_count = 0;

    // 逐行生成航点
    for (uint8_t row = 0; row < GRID_ROWS; row++)
    {
        // 分析当前行的障碍物分布
        RowAnalysis_t analysis = AnalyzeRowObstacles(row);

        if (analysis.has_obstacles)
        {
            // 有障碍物：生成详细绕行航点
            GenerateDetailedRowWaypoints(row, analysis);
        }
        else
        {
            // 无障碍物：只生成起点和终点
            GenerateSimpleRowWaypoints(row);
        }

        // 生成到下一行的转换航点（除了最后一行）
        if (row < GRID_ROWS - 1)
        {
            GenerateRowTransitionWaypoint(row, row + 1);
        }

        // 检查是否超出最大航点数
        if (flight_waypoint_count >= max_count)
        {
            break;
        }
    }

    // 复制到输出数组
    uint16_t copy_count = (flight_waypoint_count < max_count) ? flight_waypoint_count : max_count;
    for (uint16_t i = 0; i < copy_count; i++)
    {
        waypoints[i] = flight_waypoints[i];
    }

    return copy_count;
}



/**
 * @brief 简单测试函数 - 用于排查问题
 */
void SimpleWaypointTest(void)
{
    OLED_ClearDebug();
    OLED_DebugMsg("Simple Test Start");
    HAL_Delay(500);

    OLED_DebugMsg("Test 1: Basic");
    HAL_Delay(300);

    // 测试坐标转换
    float x, y;
    ConvertToDroneCoordinate(0, 0, &x, &y);
    OLED_DebugNum("X0:", (int)x);
    OLED_DebugNum("Y0:", (int)y);
    HAL_Delay(500);

    ConvertToDroneCoordinate(6, 8, &x, &y);
    OLED_DebugNum("X68:", (int)x);
    OLED_DebugNum("Y68:", (int)y);
    HAL_Delay(500);

    OLED_DebugMsg("Test 2: Add WP");
    HAL_Delay(300);

    // 测试添加航点
    flight_waypoint_count = 0; // 重置
    uint8_t result = AddFlightWaypoint(0, 0, WAYPOINT_NORMAL);
    OLED_DebugNum("Add Result:", result);
    OLED_DebugNum("Count:", flight_waypoint_count);
    HAL_Delay(500);

    OLED_DebugMsg("Simple Test Done");
}



/**
 * @brief 分析指定列的禁飞区分布
 * @param col 列号 (0-8)
 * @return 列分析结果
 */
ColAnalysis_t AnalyzeColObstacles(uint8_t col)
{
    ColAnalysis_t analysis = {0};

    if (col >= GRID_COLS)
        return analysis;

    // 扫描整列，查找禁飞区
    for (uint8_t row = 0; row < GRID_ROWS; row++)
    {
        if (!IsGridAccessible(row, col))
        {
            analysis.has_obstacles = 1;

            if (analysis.obstacle_count == 0)
            {
                analysis.first_obstacle_row = row;
            }
            analysis.last_obstacle_row = row;
            analysis.obstacle_count++;
        }
    }

    return analysis;
}

/**
 * @brief 为无障碍列生成简化航点（只有起点和终点）
 * @param col 列号
 * @return 成功返回1，失败返回0
 */
uint8_t GenerateSimpleColWaypoints(uint8_t col)
{
    if ((GRID_COLS - 1 - col) % 2 == 0) // 偶数列：从下到上 (B1→B7)
    {
        // 起点：下侧 (B1)
        if (!AddFlightWaypoint(0, col, WAYPOINT_NORMAL))
            return 0;

        // 终点：上侧 (B7)
        if (!AddFlightWaypoint(GRID_ROWS - 1, col, WAYPOINT_NORMAL))
            return 0;
    }
    else // 奇数列：从上到下 (B7→B1)
    {
        // 起点：上侧 (B7)
        if (!AddFlightWaypoint(GRID_ROWS - 1, col, WAYPOINT_NORMAL))
            return 0;

        // 终点：下侧 (B1)
        if (!AddFlightWaypoint(0, col, WAYPOINT_NORMAL))
            return 0;
    }

    return 1;
}

/**
 * @brief 为有障碍列生成详细绕行航点（修正A9列向左绕行和边缘禁飞区处理）
 * @param col 列号
 * @param analysis 列分析结果
 * @return 成功返回1，失败返回0
 */
uint8_t GenerateDetailedColWaypoints(uint8_t col, ColAnalysis_t analysis)
{
    if ((GRID_COLS - 1 - col) % 2 == 0) // 偶数列：从下到上
    {
        // 特殊情况：偶数列最上边禁飞区
        if (analysis.last_obstacle_row == GRID_ROWS - 1)
        {
            if (col == 0) // A1列B7禁飞区
            {
                // A1B7是最终终点，直接停止
                AddFlightWaypoint(0, col, WAYPOINT_NORMAL); // 起点B1
                if (analysis.first_obstacle_row > 0)
                {
                    AddFlightWaypoint(analysis.first_obstacle_row - 1, col, WAYPOINT_NORMAL); // 禁飞区前停止
                }
                return 1;
            }
            else
            {
                // 其他偶数列：向左绕行，两段式
                AddFlightWaypoint(0, col, WAYPOINT_NORMAL); // 起点B1
                AddFlightWaypoint(analysis.first_obstacle_row - 1, col, WAYPOINT_NORMAL); // 禁飞区前
                AddFlightWaypoint(analysis.first_obstacle_row - 1, col - 1, WAYPOINT_BYPASS); // 向左
                AddFlightWaypoint(GRID_ROWS - 1, col - 1, WAYPOINT_BYPASS); // 垂直到B7
                return 1; // 不回到原列
            }
        }

        // 1. 正常扫描到禁飞区前
        if (analysis.first_obstacle_row > 0)
        {
            AddFlightWaypoint(0, col, WAYPOINT_NORMAL); // 起点
            AddFlightWaypoint(analysis.first_obstacle_row - 1, col, WAYPOINT_NORMAL); // 禁飞区前
        }

        // 检查A1B7特殊情况：如果是A1列且B7是禁飞区，直接停止
        if (col == 0 && analysis.last_obstacle_row == GRID_ROWS - 1)
        {
            return 1; // 直接停止，不继续生成航点
        }

        // 2. 绕行航点序列 - 修正A9列特殊处理
        uint8_t detour_col;
        if (col == GRID_COLS - 1) // A9列特殊处理：向左绕行
        {
            detour_col = col - 1; // 向左到A8列
        }
        else if (col==0)
        {

             detour_col = col +1;
        }
        else // 其他列：向左绕行
        {
            detour_col = col - 1; // 向左绕行
        }

        // 检查绕行起点是否超出地图边界
        uint8_t bypass_start_row = analysis.first_obstacle_row - 1;
        if (bypass_start_row >= GRID_ROWS)
        {
            // 禁飞区延伸到下边缘，从禁飞区上边开始绕行
            if (analysis.last_obstacle_row < GRID_ROWS - 1)
            {
                // 直接从禁飞区上边开始绕行
                AddFlightWaypoint(analysis.last_obstacle_row + 1, detour_col, WAYPOINT_BYPASS);

                // 水平回到原列
                AddFlightWaypoint(analysis.last_obstacle_row + 1, col, WAYPOINT_BYPASS);

                // 继续正常扫描到终点
                AddFlightWaypoint(GRID_ROWS - 1, col, WAYPOINT_NORMAL); // 终点
            }
            return 1;
        }

        // 正常绕行：水平移动到绕行列
        AddFlightWaypoint(bypass_start_row, detour_col, WAYPOINT_BYPASS);

        // 垂直跳过禁飞区
        if (analysis.last_obstacle_row < GRID_ROWS - 1)
        {
            AddFlightWaypoint(analysis.last_obstacle_row + 1, detour_col, WAYPOINT_BYPASS);

            // 水平回到原列
            AddFlightWaypoint(analysis.last_obstacle_row + 1, col, WAYPOINT_BYPASS);
        }

        // 3. 继续正常扫描到终点
        if (analysis.last_obstacle_row < GRID_ROWS - 1)
        {
            AddFlightWaypoint(GRID_ROWS - 1, col, WAYPOINT_NORMAL); // 终点
        }
    }
    else // 奇数列：从上到下
    {
        // 特殊情况：奇数列最下边禁飞区
        if (analysis.first_obstacle_row == 0)
        {
            // 向左绕行，两段式
            AddFlightWaypoint(GRID_ROWS - 1, col, WAYPOINT_NORMAL); // 起点B7
            AddFlightWaypoint(analysis.last_obstacle_row + 1, col, WAYPOINT_NORMAL); // 禁飞区前
            AddFlightWaypoint(analysis.last_obstacle_row + 1, col - 1, WAYPOINT_BYPASS); // 向左
            AddFlightWaypoint(0, col - 1, WAYPOINT_BYPASS); // 垂直到B1
            return 1; // 不回到原列
        }

        // 1. 正常扫描到禁飞区前
        if (analysis.last_obstacle_row < GRID_ROWS - 1)
        {
            AddFlightWaypoint(GRID_ROWS - 1, col, WAYPOINT_NORMAL); // 起点
            AddFlightWaypoint(analysis.last_obstacle_row + 1, col, WAYPOINT_NORMAL); // 禁飞区前
        }

        // 2. 绕行航点序列 - 修正A9列特殊处理
        uint8_t detour_col;
        if (col == GRID_COLS - 1) // A9列特殊处理：向左绕行
        {
            detour_col = col - 1; // 向左到A8列
        }
        else if (col==0)
        {

            detour_col = col +1;

        }
        else // 其他列：向左绕行
        {
            detour_col = col - 1; // 向左绕行
        }

        // 检查绕行起点是否超出地图边界
        if (analysis.last_obstacle_row == GRID_ROWS - 1)
        {
            // 禁飞区延伸到上边缘，无法绕行，直接结束
            return 1;
        }

        // 水平移动到绕行列
        AddFlightWaypoint(analysis.last_obstacle_row + 1, detour_col, WAYPOINT_BYPASS);

        // 垂直跳过禁飞区
        if (analysis.first_obstacle_row > 0)
        {
            AddFlightWaypoint(analysis.first_obstacle_row - 1, detour_col, WAYPOINT_BYPASS);

            // 水平回到原列
            AddFlightWaypoint(analysis.first_obstacle_row - 1, col, WAYPOINT_BYPASS);
        }

        // 3. 继续正常扫描到终点
        if (analysis.first_obstacle_row > 0)
        {
            AddFlightWaypoint(0, col, WAYPOINT_NORMAL); // 终点
        }
    }

    return 1;
}

/**
 * @brief 生成列间转换航点（避免重复）
 * @param from_col 起始列
 * @param to_col 目标列
 * @return 成功返回1，失败返回0
 */
uint8_t GenerateColTransitionWaypoint(uint8_t from_col, uint8_t to_col)
{
    // 确定转换点的行位置
    uint8_t transition_row;

    if ((GRID_COLS - 1 - from_col) % 2 == 0) // 偶数列结束在上侧
    {
        transition_row = GRID_ROWS - 1; // B7
    }
    else // 奇数列结束在下侧
    {
        transition_row = 0; // B1
    }

    // 检查是否与下一列的起点重复
    uint8_t next_col_start_row;
    if ((GRID_COLS - 1 - to_col) % 2 == 0) // 目标列是偶数列，从下开始
    {
        next_col_start_row = 0; // B1
    }
    else // 目标列是奇数列，从上开始
    {
        next_col_start_row = GRID_ROWS - 1; // B7
    }

    // 如果转换点与下一列起点相同，则不添加转换航点（避免重复）
    if (transition_row == next_col_start_row)
    {
        return 1; // 不添加，但返回成功
    }

    // 添加转换航点
    return AddFlightWaypoint(transition_row, to_col, WAYPOINT_TRANSITION);
}//222
/**
 * @brief 生成优化的纵向巡航航点（主函数）
 * @param waypoints 输出航点数组
 * @param max_count 最大航点数
 * @return 生成的航点数量
 */
uint16_t GenerateOptimizedVerticalWaypoints(DroneWaypoint_t *waypoints, uint16_t max_count)
{
    // 重置航点计数
    flight_waypoint_count = 0;

    // 逐列生成航点
    for (int8_t col = GRID_COLS - 1; col >= 0; col--)
    {
        // 分析当前列的障碍物分布
        ColAnalysis_t analysis = AnalyzeColObstacles(col);

        if (analysis.has_obstacles)
        {
            // 有障碍物：生成详细绕行航点
            GenerateDetailedColWaypoints(col, analysis);
        }
        else
        {
            // 无障碍物：只生成起点和终点
            GenerateSimpleColWaypoints(col);
        }

        // 生成到下一列的转换航点（除了最后一列）
        if (col > 0)
        {
            GenerateColTransitionWaypoint(col, col - 1);
        }

        // 检查是否超出最大航点数
        if (flight_waypoint_count >= max_count)
        {
            break;
        }
    }

    // 复制到输出数组
    uint16_t copy_count = (flight_waypoint_count < max_count) ? flight_waypoint_count : max_count;
    for (uint16_t i = 0; i < copy_count; i++)
    {
        waypoints[i] = flight_waypoints[i];
    }

    return copy_count;
}

/**
 * @brief 纵向巡航航点生成测试函数
 */
void TestVerticalWaypointGeneration(void)
{
    OLED_ClearDebug();

    OLED_DebugMsg("Step 1: Init");

    // 1. 初始化网格地图
    InitGridMap();
    OLED_DebugMsg("Init OK");

    OLED_DebugMsg("Step 2: Set NoFly");

    // 2. 设置测试禁飞区（A5列，B2-B4三个连续格子）
    SetNoFlyZone(3, 0); // B2行，A5为禁飞区
    OLED_DebugMsg("NoFly 1 OK");

    SetNoFlyZone(4, 0); // B3行，A5为禁飞区
    OLED_DebugMsg("NoFly 2 OK");

    SetNoFlyZone(5, 0); // B4行，A5为禁飞区
    OLED_DebugMsg("NoFly 3 OK");

    // 3. 生成航点
    waypoint_count = GenerateOptimizedVerticalWaypoints(test_waypoints, MAX_FLIGHT_WAYPOINTS);
    void StartWaypointSending(uint16_t waypoint_count);

    StartWaypointSending(waypoint_count);
    // 4. 显示结果
    OLED_ClearDebug();
    OLED_DebugNum("Generated:", waypoint_count);
    jim = 456; // 区别于横向测试的标记
}


/**
 * @brief 横向巡航航点生成测试函数（智能模式切换版本）
 * @note 根据技术文档实现智能模式切换，保持向后兼容性
 */
void TestHorizontalWaypointGeneration(void)
{
   OLED_ClearDebug();

   OLED_DebugMsg("Step 1: Init");

   // 1. 初始化网格地图
   InitGridMap();
   OLED_DebugMsg("Init OK");

   OLED_DebugMsg("Step 2: Set NoFly");

   // 2. 使用存储的禁飞区数据重新设置禁飞区
   // 确保禁飞区数据已排序
   if (!nfz_sorted) {
       SortNoFlyZones();
   }

   // 根据存储的禁飞区数据设置禁飞区
   for (uint8_t i = 0; i < simple_nfz_count; i++) {
       SetNoFlyZone(simple_nfz[i].row, simple_nfz[i].col);
       OLED_DebugMsg("NoFly Set OK");
   }

   // 3. 新增：智能模式检测
 //  OLED_DebugMsg("Step 3: Analyze Mode");
   //NoFlyZoneAnalysis_t analysis = AnalyzeNoFlyZoneOrientation();

   // 显示分析结果
  // OLED_DebugNum("H-Zones:", analysis.horizontal_zones);
 //  OLED_DebugNum("V-Zones:", analysis.vertical_zones);

   // 4. 新增：根据分析结果选择生成模式
   OLED_DebugMsg("Step 4: Generate");
   if (mode==3)
   {
       OLED_DebugMsg("Mode: Vertical");
       waypoint_count = GenerateOptimizedVerticalWaypoints(test_waypoints, MAX_FLIGHT_WAYPOINTS);
      // jim = 456; // 纵向测试标记im = 123
      //jim = 123; // 横向测试标记
   }
   else if (mode==4)
   {
       OLED_DebugMsg("Mode: Horizontal");
       waypoint_count = GenerateOptimizedHorizontalWaypoints(test_waypoints, MAX_FLIGHT_WAYPOINTS);
      // jim = 123; // 横向测试标记
   }

   // 5. 启动航点发送
   StartWaypointSending(waypoint_count);

   // 6. 显示结果
   OLED_ClearDebug();
   OLED_DebugNum("Generated:", waypoint_count);
  // OLED_DebugNum("Mode:", analysis.recommended_mode);
  // OLED_DebugNum("Score:", (int)(analysis.efficiency_score * 100));
   //  // 测试：在这里加显示会卡死
   //  // 先检查waypoint_count的值是否正常
   //  if (waypoint_count > 100) // 异常值检查
   //  {
   //      OLED_ClearDebug();
   //      OLED_DebugMsg("Count ERROR!");
   //      return;
   //  }

   //  // 尝试最简单的显示
   //  HAL_Delay(500); // 给足够时间
   //  OLED_ClearDebug(); // 强制清屏
   //  OLED_DebugMsg("After count"); // 最简单的消息
   // waypoint_count++;
       // OLED_ClearDebug();

   //  HAL_Delay(300);
   //   OLED_DebugMsg("A");
   // OLED_DebugMsg("NoFly 3 OK");
   // OLED_DebugMsg("NoFly 3 OK");

   // if (waypoint_count == 0)
   // {
   //     OLED_DebugMsg("No waypoints!");
   //     return;
   // }

   // // 6. 检查内部航点数量
   // OLED_DebugNum("Internal count:", flight_waypoint_count);
   // HAL_Delay(300);

   // // 7. 安全地访问第一个航点
   // OLED_DebugMsg("Access WP 0:");
   // HAL_Delay(200);

   // // 先检查类型（整数，比较安全）
   // OLED_DebugNum("Type:", test_waypoints[0].type);
   // HAL_Delay(200);

   // // 再检查网格位置（整数）
   // OLED_DebugNum("Row:", test_waypoints[0].grid_row);
   // HAL_Delay(200);
   // OLED_DebugNum("Col:", test_waypoints[0].grid_col);
   // HAL_Delay(200);

   // // 最后尝试浮点数（可能有问题的地方）
   // OLED_DebugMsg("Try float access:");
   // HAL_Delay(200);
   

//     OLED_DebugMsg("=== Test Complete ===");
}

void StartWaypointSending(uint16_t waypoint_count)
{
    extern uint8_t waypoint_send_state;
    extern uint16_t total_waypoint_count;
    extern uint16_t current_waypoint_index;

    if (waypoint_count > 0)
    {
        total_waypoint_count = waypoint_count;
        current_waypoint_index = 0;
        waypoint_send_state = 1;  // 开始发送
    }
}

/**
 * @brief 分析禁飞区方向并推荐巡航模式
 * @return 禁飞区分析结果，包含推荐的巡航模式
 * @note 根据技术文档中的简化设计方案实现
 */
int AnalyzeNoFlyZoneOrientation(uint8_t row,uint8_t col)
{
        
     static uint8_t last_row_1,last_col_1;  
       if (row!=last_row_1)
       {
            last_row_1=row;
       }
       else 
        {
            return 1;

        }
       if(col!=last_col_1)
       {
            last_col_1=col;
       }
       else 
        {
            return 2;

        }



}